/*
  电机控制任务
*/

/* Includes ----------------------------------------------------------------- */
#include "../device/motor.h"

#include <cmsis_os2.h>

#include "../bsp/bsp_tim.h"
#include "../bsp/bsp_usart.h"
#include "../device/motor.h"
#include "../device/pid.h"
#include "usart.h"
#include "usertask.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define CONTROL_FREQ 50u
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
static Motor_t motor[4] = {{
                               .speed = 0,
                               .type = MOTOR1,
                           },
                           {
                               .speed = 0,
                               .type = MOTOR2,
                           },
                           {
                               .speed = 0,
                               .type = MOTOR3,
                           },
                           {
                               .speed = 0,
                               .type = MOTOR4,
                           }};

extern Menu_t menu;
extern Euler_t angel;
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

void MotorTask(void *argument) {
  (void)argument;
  const uint32_t delay_tick = osKernelGetTickFreq() / CONTROL_FREQ;

  Motor_Init();

  uint32_t tick = osKernelGetTickCount();
  while (1) {
    tick += delay_tick;
    // Motor_axis(motor);
    Motor_Control(angel, menu, motor);
    osDelayUntil(tick);
  }
}
